﻿// 导航逻辑地图类（寻路算法部分）

using ShawnUtils;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;

namespace ShawnNav
{
    public partial class NavMap
    {
        #region AStar算法
        // 开放队列
        readonly PriorityQueue<NavPoly> m_OpenQue = new PriorityQueue<NavPoly>();
        // 关闭列表
        readonly List<NavPoly> m_CloseLst = new List<NavPoly>();
        // 结果列表
        List<NavPoly> m_PathLst = new List<NavPoly>();

        NavPoly m_StartArea;
        NavPoly m_EndArea;

        // A星广搜算法
        List<NavPoly> CalcPolyPathByAStar(NavPoly start,  NavPoly end)
        {
            m_StartArea = start;
            m_EndArea = end;

            // 每次寻路前清空列表
            m_OpenQue.Clear();
            m_CloseLst.Clear();
            m_PathLst.Clear();

            m_OpenQue.Enqueue(start);
            start.sumDistance = 0;

            while (m_OpenQue.Count > 0)
            {
                // 找到了目标区域
                if (m_OpenQue.Contains(end))
                {
                    // 获取一整个路径
                    List<NavPoly> lst = new List<NavPoly>();
                    lst.Add(end);
                    NavPoly pre = end.prePoly;
                    while (pre != null)
                    {
                        lst.Insert(0, pre);
                        pre = pre.prePoly;
                    }
                    m_PathLst = lst;

                    int startID, endID;
                    for (int i = 0; i < lst.Count - 1; i++)
                    {
                        startID = lst[i].areaID;
                        endID = lst[i + 1].areaID;
                        string key;
                        if (startID < endID)
                            key = $"{startID}_{endID}";
                        else
                            key = $"{endID}_{startID}";

                        lst[i].targetBorder = GetBorderByAreaID(key); 
                    }

                    // 表现更新
                    ShowPathViewEvent?.Invoke(m_PathLst);
                    m_CloseLst.Add(end);
                    break;
                }

                // 没找到目标区域
                NavPoly curr = m_OpenQue.Dequeue();
                if (!m_CloseLst.Contains(curr))
                {
                    m_CloseLst.Add(curr);
                }
                for (int i = 0; i < curr.borderLst.Count; i++)
                {
                    NavBorder border = curr.borderLst[i];
                    NavPoly neighborArea =
                        curr.areaID == border.areaID1 ? polyArr[border.areaID2] : polyArr[border.areaID1];

                    DetectNeighborArea(curr, neighborArea);
                }
            }
            return m_PathLst;
        }

        void DetectNeighborArea(NavPoly curr, NavPoly neighborArea)
        {
            if (!m_CloseLst.Contains(neighborArea))
            {
                float neighborDistance = curr.CalcNavAreaDistancePoly(neighborArea);
                float newSumDistance = curr.sumDistance + neighborDistance;
                if (float.IsPositiveInfinity(neighborArea.sumDistance)
                    || newSumDistance < neighborArea.sumDistance)
                {
                    neighborArea.prePoly = curr;
                    neighborArea.sumDistance = newSumDistance;
                }

                if (!m_OpenQue.Contains(neighborArea))
                {
                    float targetDistance = neighborArea.CalcNavAreaDistancePoly(m_EndArea);
                    // f(n) = g(n) + h(n)
                    neighborArea.priority = neighborArea.sumDistance + targetDistance;
                    m_OpenQue.Enqueue(neighborArea);
                }
            }
        }
    
        void ResetAStarData()
        {
            for (int i = 0; i < m_CloseLst.Count; i++)
            {
                m_CloseLst[i].Reset();
            }
            List<NavPoly> lst = m_OpenQue.ToList();
            for (int i = 0; i < lst.Count; i++)
            {
                lst[i].Reset();
            }
        }

        #endregion

        #region Funneel算法
        enum FunnelShrinkEnum
        {
            None,
            Left2Left,      // 左向向左
            Left2Center,   // 左向向右（未越中线）
            Left2Right,     //左向向右（越过中线）
            Right2Right,    // 右向向右
            Right2Center,   // 右向向左（未越中线）
            Right2Left,     // 右向向左（越过中线）
        }

        List<NavVector3> posLst = null;
        NavVector3 funnelPos = NavVector3.Zero;

        readonly List<int> leftConnerLst = new List<int>();
        readonly List<int> rightConnerLst = new List<int>();  

        // 当前漏斗计算点位索引
        int curLeftIndex = -1;
        int curRightIndex = -1;
        // 漏斗极限点位索引
        int leftLimitIndex = -1;
        int rightLimitIndex = -1;
        // 漏斗极限向量
        NavVector3 leftLimitDir = NavVector3.Zero;
        NavVector3 rightLimitDir = NavVector3.Zero;
        // 下一次待检测索引
        int leftCheckIndex = -1;
        int rightCheckIndex = -1;
        NavVector3 leftCheckDir = NavVector3.Zero;
        NavVector3 rightCheckDir = NavVector3.Zero;

        List<NavVector3> CalcFunnelConnerPath(List<NavPoly> areaLst, NavVector3 start, NavVector3 end)
        {
            posLst = new List<NavVector3> { start };
            funnelPos = start;

            // 清除容器数据
            leftConnerLst.Clear();
            rightConnerLst.Clear();

            // 初始化漏斗Funnel
            int initIndex = CalcInitAreaID(areaLst);
            if (initIndex == -1)
            {
                posLst.Add(end);
                return posLst;
            }
            this.LogCyan($"initAreaID:{areaLst[initIndex].areaID}");

            FunnelShrinkEnum leftFSE, rightFSE;
            for (int i = initIndex + 1; i < areaLst.Count; i++)
            {
                NavPoly area = areaLst[i];
                if (i == areaLst.Count - 1)
                {
                    NavVector3 endVector = end - funnelPos;
                    leftCheckDir = endVector;
                    rightCheckDir = endVector;

                    bool inLeft = NavVector3.CrossXZ(leftLimitDir, endVector) > 0;
                    bool inRight = NavVector3.CrossXZ(rightLimitDir, endVector) < 0;

                    if (inLeft)
                    {
                        CalcEndConner(leftConnerLst, leftLimitIndex, leftLimitDir, end);
                    }
                    else if (inRight)
                    {
                        CalcEndConner(rightConnerLst, rightLimitIndex, rightLimitDir, end);
                    }
                    else
                    {
                        posLst.Add(end);
                    }
                }
                else
                {
                    CalcCheckFunnel(area);
                    leftFSE = CalcLeftFunnelType();
                    rightFSE = CalcRightFunnelType();

                    if (leftFSE == FunnelShrinkEnum.Left2Left)
                    {
                        // 左向向左，缓存节点
                        if (!leftConnerLst.Contains(leftCheckIndex))
                        {
                            leftConnerLst.Add(leftCheckIndex);
                        }
                    }
                    if (rightFSE == FunnelShrinkEnum.Right2Right)
                    {
                        // 右向向右，缓存节点
                        if (!rightConnerLst.Contains(rightCheckIndex))
                        {
                            rightConnerLst.Add(rightCheckIndex);
                        }
                    }

                    # region Left
                    switch (leftFSE)
                    {
                        case FunnelShrinkEnum.None:
                            leftLimitIndex = leftCheckIndex;
                            break;
                        case FunnelShrinkEnum.Left2Center:
                            // 收缩漏斗，清除数据缓存
                            leftLimitIndex = leftCheckIndex;
                            leftLimitDir = leftCheckDir;
                            leftConnerLst.Clear();
                            break;
                        case FunnelShrinkEnum.Left2Right:
                            CalcLeft2RightLimitIndex();
                            break;
                    }
                    #endregion

                    # region Right
                    switch (rightFSE)
                    {
                        case FunnelShrinkEnum.None:
                            rightLimitIndex = rightCheckIndex;
                            break;
                        case FunnelShrinkEnum.Right2Center:
                            // 收缩漏斗，清除数据缓存
                            rightLimitIndex = rightCheckIndex;
                            rightLimitDir = rightCheckDir;
                            rightConnerLst.Clear();
                            break;
                        case FunnelShrinkEnum.Right2Left:
                            CalcRight2LeftLimitIndex();
                            break;
                    }
                    #endregion

                    curLeftIndex = leftCheckIndex;
                    curRightIndex = rightCheckIndex;
                }
            }
            return posLst;
        }

        // 计算初始化位置节点
        int CalcInitAreaID(List<NavPoly> areaLst)
        {
            int initAreaID = -1;
            if (areaLst.Count == 0)
                return initAreaID;

            
            for (int i = 0; i < areaLst.Count;i++)
            {
                if (IsFunnelInitArea(areaLst[i]))
                {
                    initAreaID = i;
                    break;
                }
            }
            return initAreaID;
        }

        // 判断当前区块是否是初始化区块
        bool IsFunnelInitArea(NavPoly area)
        {
            if (area.targetBorder == null)
                return false;

            int index1 = area.targetBorder.verticeIndex1;
            int index2 = area.targetBorder.verticeIndex2;
            NavVector3 v1 = verticeArr[index1] - funnelPos;
            NavVector3 v2 = verticeArr[index2] - funnelPos;

            // NavView.ShowDebugLine(funnelPos, verticeArr[index1], UnityEngine.Color.cyan, 10);
            // NavView.ShowDebugLine(funnelPos, verticeArr[index2], UnityEngine.Color.cyan, 10);

            float crossxz = NavVector3.CrossXZ(v1, v2);
            if (crossxz < 0)
            {
                // v2在v1的顺时针,对漏斗参数做初始化
                curLeftIndex = index1;
                curRightIndex = index2;
                leftLimitIndex = index1;
                rightLimitIndex = index2;
                leftLimitDir = v1;
                rightLimitDir = v2;
                return true;
            }else if (crossxz > 0)
            {
                // v2在v1的逆时针
                curLeftIndex = index2;
                curRightIndex = index1;
                leftLimitIndex = index2;
                rightLimitIndex = index1;
                leftLimitDir = v2;
                rightLimitDir = v1;
                return true;
            }
            else
            {
                // v2 与 v1共线
                this.Warn($"初始向量共线：arae:{area.areaID} border:{area.targetBorder.verticeIndex1}_{area.targetBorder.verticeIndex2} funnelPos:{funnelPos}");
                return false;
            }
        }
        
        // 计算 待检测 的索引与向量
        void CalcCheckFunnel(NavPoly area)
        {
            int checkIndex1 = area.targetBorder.verticeIndex1;
            int checkIndex2 = area.targetBorder.verticeIndex2;
            NavVector3 checkV1 = verticeArr[checkIndex1] - funnelPos;
            NavVector3 checkV2 = verticeArr[checkIndex2] - funnelPos;
            #region DebugShow
            // NavView.ShowDebugLine(funnelPos, verticeArr[checkIndex1], UnityEngine.Color.cyan, 10);
            // NavView.ShowDebugLine(funnelPos, verticeArr[checkIndex2], UnityEngine.Color.cyan, 10);
            #endregion
            int offset = 0;
            int count = area.indexArr.Length;
            for (int j = 0; j < count; j++)
            {
                if (curLeftIndex == area.indexArr[j])
                {
                    offset = j;
                    break;
                }
            }
            for (int j = 0; j < count; j++)
            {
                int curIndex = area.indexArr[(j + offset) % count];
                if (curIndex == checkIndex1)
                {
                    leftCheckIndex = checkIndex1;
                    leftCheckDir = checkV1;
                    rightCheckIndex = checkIndex2;
                    rightCheckDir = checkV2;
                    break;
                }
                else if (curIndex == checkIndex2)
                {
                    leftCheckIndex = checkIndex2;
                    leftCheckDir = checkV2;
                    rightCheckIndex = checkIndex1;
                    rightCheckDir = checkV1;
                    break;
                }
                else
                {
                    // this.Log($"  {j + offset}");
                }
            }

            //
            if (leftLimitDir == NavVector3.Zero)
                leftLimitDir = leftCheckDir;
            if (rightLimitDir == NavVector3.Zero)
                rightLimitDir = rightCheckDir;
        }

        // 计算左边漏斗的类型
        FunnelShrinkEnum CalcLeftFunnelType()
        {
            FunnelShrinkEnum leftFSE = FunnelShrinkEnum.None;
            float ll = NavVector3.CrossXZ(leftLimitDir, leftCheckDir);
            if (ll > 0)
            {
                // 待监测点 在 极限点的左边
                leftFSE = FunnelShrinkEnum.Left2Left;
            }
            else if (ll == 0)
            {
                // 待检测 与 极限点共线
                leftFSE = FunnelShrinkEnum.None;
            }
            else
            {
                float lr = NavVector3.CrossXZ(rightLimitDir, leftCheckDir);
                if (lr > 0)
                {
                    leftFSE = FunnelShrinkEnum.Left2Center;
                }
                else
                {
                    leftFSE = FunnelShrinkEnum.Left2Right;
                }
            }
            return leftFSE;
        }

        // 计算右边漏斗的类型
        FunnelShrinkEnum CalcRightFunnelType()
        {
            FunnelShrinkEnum rightFSE = FunnelShrinkEnum.None;
            float rr = NavVector3.CrossXZ(rightLimitDir, rightCheckDir);
            if (rr < 0)
            {
                // 待监测点 在 极限点的左边
                rightFSE = FunnelShrinkEnum.Right2Right;
            }
            else if (rr == 0)
            {
                // 待检测 与 极限点共线
                rightFSE = FunnelShrinkEnum.None;
            }
            else
            {
                float rl = NavVector3.CrossXZ(leftLimitDir, rightCheckDir);
                if (rl < 0)
                {
                    rightFSE = FunnelShrinkEnum.Right2Center;
                }
                else
                {
                    rightFSE = FunnelShrinkEnum.Right2Left;
                }
            }
            return rightFSE;
        }

        // 计算 左向向右 越过右极限点情况的逻辑处理
        void CalcLeft2RightLimitIndex()
        {
            // 移动漏斗顶点
            funnelPos = verticeArr[rightLimitIndex];
            posLst.Add(funnelPos);

            // 标志是否进入while循环
            bool updateLimit = false;

            // 计算拐点偏转角度最小值
            int connerIndex = 0;
            NavVector3 rldn = NavVector3.NormalXZ(rightLimitDir);
            while (connerIndex < rightConnerLst.Count)
            {
                float rad = float.MaxValue;
                for (int i = connerIndex; i < rightConnerLst.Count; i++)
                {
                    NavVector3 ckdn = NavVector3.NormalXZ(verticeArr[rightConnerLst[i]] - funnelPos);
                    float curRad = MathF.Abs(NavVector3.AngleXZ(rldn, ckdn));
                    if (curRad <= rad)
                    {
                        connerIndex = i;
                        rad = curRad;
                    }
                }

                updateLimit = true;

                // 更新漏斗位置极限
                rightLimitIndex = rightConnerLst[connerIndex];
                rightLimitDir = verticeArr[rightLimitIndex] - funnelPos;
                leftLimitIndex = leftCheckIndex;
                leftLimitDir = verticeArr[leftLimitIndex] - funnelPos;

                float cross = NavVector3.CrossXZ(leftLimitDir, rightLimitDir);
                if (cross > 0)
                {
                    // v2的v1的逆时针方向，漏斗不合法，移动漏斗
                    funnelPos = verticeArr[rightLimitIndex];
                    posLst.Add(funnelPos);
                    ++connerIndex;
                    // 特殊情况
                    if (connerIndex >= rightConnerLst.Count)
                    {
                        rightLimitIndex = -1;
                        rightLimitDir = NavVector3.Zero;

                        leftLimitDir = verticeArr[leftLimitIndex] - funnelPos;
                    }
                }
                else
                {
                    //v2在v1的顺时针方向
                    for (int i = 0; i < connerIndex; i++)
                    {
                        rightConnerLst.RemoveAt(0);
                    }
                    break;
                }
            }
        
            if (!updateLimit)
            {
                rightLimitIndex = -1;
                rightLimitDir = NavVector3.Zero;
                leftLimitIndex = leftCheckIndex;
                leftLimitDir = verticeArr[leftLimitIndex] - funnelPos;
            }    
        }

        // 计算 左向向右 越过右极限点情况的逻辑处理
        void CalcRight2LeftLimitIndex()
        {
            funnelPos = verticeArr[leftLimitIndex];
            posLst.Add(funnelPos);

            bool updateLimit = false;
            int connerIndex = 0;
            NavVector3 lldn = NavVector3.NormalXZ(leftLimitDir);
            while (connerIndex < leftConnerLst.Count)
            {
                float rad = float.MaxValue;
                for (int i = connerIndex; i < leftConnerLst.Count; i++)
                {
                    NavVector3 ckdn = NavVector3.NormalXZ(verticeArr[leftConnerLst[i]] - funnelPos);
                    float curRad = MathF.Abs(NavVector3.AngleXZ(lldn, ckdn));
                    if (curRad <= rad)
                    {
                        connerIndex = i;
                        rad = curRad;
                    }
                }

                updateLimit = true;
                leftLimitIndex = leftConnerLst[connerIndex];
                leftLimitDir = verticeArr[leftLimitIndex] - funnelPos;

                rightLimitIndex = rightCheckIndex;
                rightLimitDir = verticeArr[rightLimitIndex] - funnelPos;
                float cross = NavVector3.CrossXZ(leftLimitDir, rightLimitDir);
                if (cross > 0)
                {
                    funnelPos = verticeArr[leftLimitIndex];
                    posLst.Add(funnelPos);
                    ++connerIndex;
                    if (connerIndex >= leftConnerLst.Count)
                    {
                        leftLimitIndex = -1;
                        leftLimitDir = NavVector3.Zero;

                        rightLimitDir = verticeArr[rightLimitIndex] - funnelPos;
                    }
                }
                else
                {
                    for (int i = 0; i < connerIndex + 1; i++)
                    {
                        leftConnerLst.RemoveAt(0);
                    }
                    break;
                }
            }

            if (!updateLimit)
            {
                leftLimitIndex = -1;
                leftLimitDir = NavVector3.Zero;

                rightLimitIndex = rightCheckIndex;
                rightLimitDir = verticeArr[rightLimitIndex] - funnelPos;
            }
        }

        void CalcEndConner(List<int> connerIndexLst, int limitIndex, NavVector3 limitDir, NavVector3 end)
        {
            funnelPos = verticeArr[limitIndex];
            posLst.Add(funnelPos);
            List<NavVector3> connerPosLst = new List<NavVector3>();
            for (int i = 0; i < connerIndexLst.Count; i++)
            {
                connerPosLst.Add(verticeArr[connerIndexLst[i]]); 
            }
            bool isExist = false;
            for (int i = 0; i < connerPosLst.Count; i++)
            {
                if (connerPosLst[i] == end)
                {
                    isExist = true;
                    break;
                }
            }
            if (!isExist)
                connerPosLst.Add(end);

            NavVector3 ln = NavVector3.NormalXZ(limitDir);
            int connerIndex = 0;
            while (connerIndex < connerPosLst.Count)
            {
                float rad = float.MaxValue;
                for (int j = connerIndex; j < connerPosLst.Count; j++)
                {
                    NavVector3 checkVec = connerPosLst[j] - funnelPos;
                    NavVector3 ckn = NavVector3.NormalXZ(checkVec);
                    float curRad = MathF.Abs(NavVector3.AngleXZ(ln, ckn));
                    if (curRad < rad)
                    {
                        rad = curRad;
                        connerIndex = j;
                    }
                }
                funnelPos = connerPosLst[connerIndex];
                posLst.Add(funnelPos);
                connerIndex++;
            }
        }

        void ResetFunnelData()
        {
            curLeftIndex = -1;
            curRightIndex = -1;
            leftLimitIndex = -1;
            rightLimitIndex = -1;
            leftLimitDir = NavVector3.Zero;
            rightLimitDir = NavVector3.Zero;
            leftCheckIndex = -1;
            rightCheckIndex = -1;
            leftCheckDir = NavVector3.Zero;
            rightCheckDir = NavVector3.Zero;
        }
        #endregion
    }
}
